#!/usr/bin/env python

# Code for Jetson Nano on drone

import paho.mqtt.client as mqtt
import json
import string

import subprocess
import time

import os

from polytest import plan_coverage_path

# Global variable to store the latest GPS data
 
def on_connect(client, userdata, flags, rc):
    print(f"Connected with result code {rc}")
    
    client.subscribe("/k0rleMYIfq9/RasberryPie/user/DroneFromApp")
 
 
def on_message(client, userdata, msg):
    json_data = json.loads(msg.payload)
    msgcode = json_data['msgcode']
    #droneid = json_data['droneid']
    #userid = json_data['userid']
	# Define operation code to container operations
    if msgcode == "invoke_script":
		# python code here
        print(f"Message code is {msgcode} received.")
        #print(f"Now invoking taking off script, the taking off drone is {droneid}, user is {userid}.")
        
        invoke()
        
    elif msgcode == "waypoints":
        #print(f"[Waypoints] Received from user {userid} for drone {droneid}")

        points = json_data.get("points", [])
        if not points:
            print("No points received!")
            return

        # 分别提取纬度和经度
    lat_list = [p["lat"] for p in points]
    lon_list = [p["lng"] for p in points]

    print(f"✅ 接收到 {len(lat_list)} 个航点坐标。")
    print("lat_list =", lat_list)
    print("lon_list =", lon_list)
    
    # TODO: 在此调用你飞控接口进行下一步操作
    
    result = plan_coverage_path(
        lat_list=lat_list,
        lon_list=lon_list,
        fov_deg=75.0,
        altitude_m=8.0,
        overlap_ratio=0.9,
        utm_zone=None,      # 如果你知道区域可以填写
        plot=True,          # 是否绘图显示
        csv_path="../src/waypoints.csv"  # 输出 CSV，可为空
    )

    print("路径规划完成")

def invoke():
    
    # 切换到 catkin_ws 目录
    #os.chdir(os.path.expanduser("~/catkin_ws"))
    
    # 加载 ROS 环境变量
    subprocess.run("source ~/.bashrc", shell=True, executable='/bin/bash')
    print("Source environment")
    
    # 修改串口权限
    subprocess.run("sudo chmod 777 /dev/ttyUSB0", shell=True)
    
    # 启动 MAVROS
    mavros_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "roslaunch mavros apm.launch; exec bash"]
    )
    time.sleep(5)  # 确保 MAVROS 启动完成
    
    # 设置 MAVROS 速率
    #command=["rosrun", "mavros", "mavsys", "rate", "--all", "10"]
    
    rate_process = subprocess.Popen(["gnome-terminal", "--", "bash", "-c", "sleep 2; rosrun mavros mavsys rate --all 10; exec bash"])
    print("rate_change")
    
    # 启动 AR 视觉跟踪
    ar_tracking_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "sleep 2; roslaunch yahboomcar_visual ar_track_csi.launch; exec bash"]
    )
    time.sleep(4)  # 确保 AR 追踪启动
    
    # 启动 GPS + 视觉降落
    gps_landing_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "sleep 6; rosrun iq_gnc 124GPSvisionland; exec bash"]
    )
    
    print("All ROS nodes have been launched successfully.")
    
    return

if __name__ == '__main__':
    
    client = mqtt.Client("k0rleMYIfq9.Drone|securemode=2,signmethod=hmacsha256,timestamp=1741590381877|",transport='tcp') 
    client.username_pw_set("Drone&k0rleMYIfq9",password="e170e2493093ef27b655d874632a7ae5e377df0f3d2628e34537a07c1bde8fbc")
    client.on_connect = on_connect
    client.on_message = on_message
    
    client.connect("iot-06z00e244qqpnlb.mqtt.iothub.aliyuncs.com", 1883, 60)
    client.loop_forever()
    
